package edu.wpi.first.wpilibj.templates;

import com.sun.squawk.io.BufferedWriter;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.OutputStreamWriter;
import java.util.Timer;
import java.util.TimerTask;
import javax.microedition.io.Connector;
import javax.microedition.io.SocketConnection;

/**
 *
 * @author Kareem El-Faramawi
 */
public class Dash {
	private static final String host = "10.1.51.9";
	private static final int port = 1735;
	private SocketConnection socket;
	private InputStreamReader reader;
	private BufferedWriter writer;
	public static double camTurnHoriz = 0;
	public static double camTurnVert = 0;

	public Dash() throws IOException {
		socket = ( SocketConnection ) Connector.open( "socket://" + host + ":" + port );
		reader = new InputStreamReader( socket.openDataInputStream() );
		writer = new BufferedWriter( new OutputStreamWriter( socket.openDataOutputStream() ) );
		new Thread() {
			public void run() {
				while( true ) {
					try {
						if ( reader.ready() ) {
							int length = reader.read();
							int offset = 0;
							char[] buffer = new char[length];
							while ( length != 0 ) {
								int read = reader.read( buffer, offset, length );
								offset += read;
								length -= read;
								if ( read == -1 ) {
									break;
								}
							}
							String msg = String.valueOf( buffer );
							try {
								String horiz = msg.substring( 0, msg.indexOf( ':'));
								String vert = msg.substring( msg.indexOf( ':' ) + 1);
								camTurnHoriz = Double.parseDouble( horiz );
								camTurnVert = Double.parseDouble( vert );
								System.out.println( horiz + "\t" + vert );
								send( "DR" );
							} catch ( NumberFormatException ex ) {
								System.err.println( "FAILED TO PARSE MESSAGE" );
								//ex.printStackTrace();
							}
						}
					} catch ( IOException ex ) {
						System.err.println( "CANNOT SEND REPLY" );
						//ex.printStackTrace();
					}
				}
			}
		}.start();
	}

	public void testConnection() throws IOException {
		send( "T" );
	}

	public void send( String msg ) throws IOException {
		writer.write( msg.length() );
		writer.write( msg );
		writer.flush();
	}
}
